package main;

import java.io.IOException;

import bluetooth.BTReceive;
import lejos.nxt.ColorSensor;
import lejos.nxt.ColorSensor.Color;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.RegulatedMotor;

public class NXTFusionTest {
	
	
	private static RegulatedMotor leftMotor = Motor.A;
	private static RegulatedMotor rightMotor = Motor.C;

	private static ColorSensor cs = new ColorSensor(SensorPort.S1);
	private static UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);

	
	public static void main(String[] args) throws IOException, InterruptedException {
		
		leftMotor.setSpeed(50);
		rightMotor.setSpeed(50);
		
		LCD.clear();
		LCD.drawString("Clique para iniciar", 3, 4);
		LCD.refresh();

		while (!Button.ESCAPE.isDown()) {

		}
		
		BTReceive btReceive = new BTReceive();
		btReceive.connect();
		Color color = cs.getColor();
		btReceive.write(new byte[]{(byte) color.getRed(),(byte) color.getGreen(),(byte) color.getBlue(), (byte) sonic.getDistance()});
		
		byte[] readValues = btReceive.read();
		move(readValues);
		
	}


	private static void move(byte[] readValues) throws InterruptedException {
		leftMotor.rotate(readValues[0], true);
		rightMotor.rotate(readValues[1], true);
		Thread.sleep(200);
		
	}
	

}
